#include "spwm.h"
#include "lib/fastTrig.h"
#include <cmath>
#include "main.h"

static int16_t m_spwmSinTable[SPWM_MODULATE_SIN_MAX];
static bool m_spwmTableIsInit = false;

SPwm::SPwm()
{
}

SPwm::SPwm(SOC_Timer *timer, int ch, float ma)
{
    Config(timer, ch, ma);
}

SPwm::~SPwm()
{
}

void SPwm::Config(SOC_Timer *timer, int ch, float ma)
{
    m_timer = timer;
    m_ch = ch;
    m_mA = ma;

    if(m_mA > 1)
        m_mA = 1;
    
    m_halfPeriod = m_timer->GetPeriod() / 2;
}

void SPwm::Start()
{
    FW_ASSERT(m_timer);

    m_timer->StartPwm(m_ch);
}

void SPwm::Stop()
{
    FW_ASSERT(m_timer);

    m_timer->StopPwm(m_ch);
}

void SPwm::SetPeriod(int period)
{
    FW_ASSERT(m_timer);

    m_timer->SetPeriod(period);
}

void SPwm::SetDuty(float duty)
{
    FW_ASSERT(m_timer);

    m_timer->SetPwmDuty(m_ch, duty);
}

void SPwm::SetCompare(int val)
{
    FW_ASSERT(m_timer);

    m_timer->SetCompare(m_ch, val);
}

void SPwm::UpdateToPwm()
{
    m_timer->SetCompare(m_ch, m_lastCp);
}

int SPwm::GetLastCompare()
{
    return m_lastCp;
}

void SPwm::InitTable(int maxPeriod)
{
    if(m_spwmTableIsInit)
        return;
    if(maxPeriod == 0)
        return;

    m_spwmTableIsInit = true;

    int halfPeriod = maxPeriod / 2;
    for (int i = 0; i < SPWM_MODULATE_SIN_MAX; i++)
    {
        float x = i * M_TWOPI / SPWM_MODULATE_SIN_MAX;
        float val = sinf(x);
        m_spwmSinTable[i] = (int16_t)round(halfPeriod * val);
    }
}

void SPwm::SetAngle(float angle)
{
    // FW_ASSERT(m_timer);
    
#if true
    // int x = angle / 360.0 * 400;
    int x = angle * 1.111111111;
    int16_t val;

    if(x >= 0)
    {
        x = x % SPWM_MODULATE_SIN_MAX;
        val = m_spwmSinTable[x];
    }
    else
    {
        x = (-x) % SPWM_MODULATE_SIN_MAX;
        val = -m_spwmSinTable[x];
    }
    // int cp = m_spwmSinTable[x] * m_mA + m_halfPeriod;
    int cp = (val + m_halfPeriod) * m_mA;
#else
    int period = m_timer->GetPeriod();
    // int cp = (FastSinByAngle(angle) + 1) * m_mA * (period / 2.0f);
    int cp = (int)(FastSinByAngle(angle) * 800 + 0.5F) + (period / 2);
    cp = cp % period;
#endif

    m_lastCp = cp;
}

void SPwm::SetIndex(int index)
{
    if(index < 0)
        index = 0;

    index = index % SPWM_MODULATE_SIN_MAX;
    
    int cp = m_spwmSinTable[index] * m_mA;
    m_lastCp = cp;
}
